/*
 * ode78_.c
 *
 * Code generation for function 'ode78_'
 *
 * C source code generated on: Mon Jun 22 23:07:02 2015
 *
 */

/* Include files */
#include "rt_nonfinite.h"
#include "ode78_.h"
#include "fy_.h"
#include "ode78__data.h"

/* Variable Definitions */
static emlrtRSInfo emlrtRSI = { 87, "ode78_",
  "C:/\xe4\xba\x91\xe7\x9b\x98\xe5\x90\x8c\xe6\xad\xa5\xe6\x96\x87\xe4\xbb\xb6\xe5\xa4?/GTOC8/Lowthrust_mee_v0/ode78_.m"
};

static emlrtRSInfo b_emlrtRSI = { 89, "ode78_",
  "C:/\xe4\xba\x91\xe7\x9b\x98\xe5\x90\x8c\xe6\xad\xa5\xe6\x96\x87\xe4\xbb\xb6\xe5\xa4?/GTOC8/Lowthrust_mee_v0/ode78_.m"
};

static emlrtRSInfo c_emlrtRSI = { 129, "ode78_",
  "C:/\xe4\xba\x91\xe7\x9b\x98\xe5\x90\x8c\xe6\xad\xa5\xe6\x96\x87\xe4\xbb\xb6\xe5\xa4?/GTOC8/Lowthrust_mee_v0/ode78_.m"
};

static emlrtRSInfo i_emlrtRSI = { 37, "mpower",
  "C:/Program Files/MATLAB/R2013a/toolbox/eml/lib/matlab/ops/mpower.m" };

static emlrtRSInfo j_emlrtRSI = { 42, "power",
  "C:/Program Files/MATLAB/R2013a/toolbox/eml/lib/matlab/ops/power.m" };

static emlrtRSInfo k_emlrtRSI = { 56, "power",
  "C:/Program Files/MATLAB/R2013a/toolbox/eml/lib/matlab/ops/power.m" };

/* Function Declarations */
static void b_eml_error(void);

/* Function Definitions */
static void b_eml_error(void)
{
  emlrtPushRtStackR2012b(&h_emlrtRSI, emlrtRootTLSGlobal);
  emlrtErrorWithMessageIdR2012b(emlrtRootTLSGlobal, &emlrtRTEI,
    "Coder:toolbox:power_domainError", 0);
  emlrtPopRtStackR2012b(&h_emlrtRSI, emlrtRootTLSGlobal);
}

void ode78_(const real_T tspan[2], const real_T x0[14], const struct_T *auxdata,
            real_T *tout, real_T xout[14], real_T *ErrorFlag)
{
  real_T hmax;
  real_T h;
  real_T t;
  int32_T fk;
  real_T x[14];
  real_T f[182];
  int32_T i;
  int32_T i0;
  boolean_T exitg1;
  real_T b[13];
  static const real_T a_i_j[156] = { 0.055555555555555552, 0.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.020833333333333332, 0.0625, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03125, 0.0, 0.09375, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3125, 0.0, -1.171875,
    1.171875, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0375, 0.0, 0.0,
    0.1875, 0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.047910137111111112,
    0.0, 0.0, 0.11224871277777777, -0.025505673777777779, 0.012846823888888888,
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.016917989787292281, 0.0, 0.0,
    0.3878482784860432, 0.035977369851500331, 0.19697021421566607,
    -0.17271385234050185, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0690957533591923, 0.0,
    0.0, -0.63424797672885413, -0.16119757522460407, 0.13865030945882525,
    0.94092861403575623, 0.21163632648194397, 0.0, 0.0, 0.0, 0.0, 0.0,
    0.18355699683904539, 0.0, 0.0, -2.4687680843155926, -0.29128688781630047,
    -0.026473020233117376, 2.8478387641928005, 0.28138733146984979,
    0.12374489986331466, 0.0, 0.0, 0.0, 0.0, -1.2154248173958881, 0.0, 0.0,
    16.672608665945774, 0.915741828416818, -6.0566058043574706,
    -16.00357359415618, 14.849303086297663, -13.371575735289849,
    5.134182648179638, 0.0, 0.0, 0.0, 0.25886091643826425, 0.0, 0.0,
    -4.7744857854892047, -0.43509301377703252, -3.0494833320722416,
    5.5779200399360995, 6.15583158986104, -5.0621045867369387, 2.193926173180679,
    0.13462799865933495, 0.0, 0.0, 0.82242759962650747, 0.0, 0.0,
    -11.658673257277664, -0.75762211669093615, 0.71397358815958156,
    12.075774986890057, -2.1276591139204029, 1.9901662070489554,
    -0.23428647154404028, 0.17589857770794226, 0.0, 0.0 };

  real_T b_x[14];
  real_T scale;
  int32_T i1;
  real_T sol2[14];
  static const real_T b_b[13] = { 0.041747491141530244, 0.0, 0.0, 0.0, 0.0,
    -0.055452328611239311, 0.23931280720118009, 0.703510669403443,
    -0.75975961381446089, 0.6605630309222863, 0.15818748251012332,
    -0.23810953875286281, 0.25 };

  static const real_T c_b[13] = { 0.0295532136763535, 0.0, 0.0, 0.0, 0.0,
    -0.828606276487797, 0.31124090005111832, 2.4673451905998869,
    -2.5469416518419088, 1.4435485836767752, 0.079415595881127288,
    0.044444444444444446, 0.0 };

  real_T error_1;
  real_T absx;
  real_T b_t;
  boolean_T exitg2;

  /*  ------------------------------------------------------------------ */
  /*  rk78 integration */
  /*  */
  /*  [tout, xout, ErrorFlag] = ode78_chen_(tspan , x0 , auxdata) */
  /*  */
  /*  Input argumuents: */
  /*  ------------------------------------------------------------------------- */
  /*  tspan            [1x2 double]           time array                                  [ TU ]         */
  /*  x0                 [14x1 double]         state and costate variables     [  -  ] */
  /*  */
  /*  Output argumuents: */
  /*  ------------------------------------------------------------------------- */
  /*  tout              [1x1 double]            position vector          [  -  ] */
  /*  xout             [14x1 double]          velocity vector           [  -  ] */
  /*  ErrorFlag   [1x1 double]             error flag                   [  -  ] */
  /*  */
  /*  External functions called: */
  /*  ------------------------------------------------------------------------- */
  /*  fy_.m */
  /*  */
  /*  Copyright (C) 2015/06/22 by Chen Zhang,  */
  /*  BeiHang University, chenzhang.buaa@gmail.com */
  /*  ------------------------------------------------------------------ */
  /*  The coefficients of rk78 */
  /*  power for step control */
  /*  Maximum step size */
  hmax = (tspan[1] - tspan[0]) / 2.5;

  /*  Initial step size */
  h = hmax;
  if (hmax > 0.5) {
    h = 0.5;
  }

  if (h > hmax) {
    h = hmax;
  }

  /*  Relative error tolerance (applies to all components of the solution vector) */
  t = tspan[0];

  /*  Minimum step size */
  /*  Initialization */
  /*  constant for step rejection */
  /*  counter for function evaluations */
  fk = 0;
  *ErrorFlag = 0.0;

  /*  array f for RHS calculation */
  *tout = tspan[0];
  for (i = 0; i < 14; i++) {
    x[i] = x0[i];

    /*  start point */
    for (i0 = 0; i0 < 13; i0++) {
      f[i + 14 * i0] = x0[i] * 0.0;
    }

    xout[i] = x0[i];
  }

  /*  tau = tol * max(norm(x,'inf'), 1) ;  % accuracy */
  /*  The main loop */
  /*  count = 1; */
  exitg1 = FALSE;
  while ((exitg1 == FALSE) && ((t < tspan[1]) && (h >= 3.5527136788005009E-15 *
           muDoubleScalarAbs(tspan[0])))) {
    if (t + h > tspan[1]) {
      h = tspan[1] - t;
    }

    /*  Compute the RHS for step of method */
    emlrtPushRtStackR2012b(&emlrtRSI, emlrtRootTLSGlobal);
    fy_(x, auxdata->mu, auxdata->Tmax, auxdata->c, *(real_T (*)[14])&f[0]);
    emlrtPopRtStackR2012b(&emlrtRSI, emlrtRootTLSGlobal);
    for (i = 0; i < 12; i++) {
      emlrtPushRtStackR2012b(&b_emlrtRSI, emlrtRootTLSGlobal);
      memcpy(&b[0], &a_i_j[13 * i], 13U * sizeof(real_T));
      for (i0 = 0; i0 < 14; i0++) {
        scale = 0.0;
        for (i1 = 0; i1 < 13; i1++) {
          scale += h * f[i0 + 14 * i1] * b[i1];
        }

        b_x[i0] = x[i0] + scale;
      }

      fy_(b_x, auxdata->mu, auxdata->Tmax, auxdata->c, *(real_T (*)[14])&f[14 *
          (1 + i)]);
      emlrtPopRtStackR2012b(&b_emlrtRSI, emlrtRootTLSGlobal);
      emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, emlrtRootTLSGlobal);
    }

    /* %%%%%%%%% Forced exit condition %%%%%%%%%% */
    fk += 13;
    if (fk > 50000) {
      *ErrorFlag = 1.0;
      exitg1 = TRUE;
    } else {
      /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
      /*  Two solution */
      for (i0 = 0; i0 < 14; i0++) {
        scale = 0.0;
        for (i1 = 0; i1 < 13; i1++) {
          scale += h * f[i0 + 14 * i1] * b_b[i1];
        }

        sol2[i0] = x[i0] + scale;
      }

      /*  8-th order solution */
      /*  7-th order solution */
      /*  Truncation error */
      for (i0 = 0; i0 < 14; i0++) {
        scale = 0.0;
        for (i1 = 0; i1 < 13; i1++) {
          scale += h * f[i0 + 14 * i1] * c_b[i1];
        }

        b_x[i0] = (x[i0] + scale) - sol2[i0];
      }

      error_1 = 0.0;
      scale = 2.2250738585072014E-308;
      for (i = 0; i < 14; i++) {
        absx = muDoubleScalarAbs(b_x[i]);
        if (absx > scale) {
          b_t = scale / absx;
          error_1 = 1.0 + error_1 * b_t * b_t;
          scale = absx;
        } else {
          b_t = absx / scale;
          error_1 += b_t * b_t;
        }
      }

      error_1 = scale * muDoubleScalarSqrt(error_1);

      /*  Estimate the error and the acceptable error */
      scale = 0.0;
      i = 0;
      exitg2 = FALSE;
      while ((exitg2 == FALSE) && (i < 14)) {
        absx = muDoubleScalarAbs(x[i]);
        if (muDoubleScalarIsNaN(absx)) {
          scale = rtNaN;
          exitg2 = TRUE;
        } else {
          if (absx > scale) {
            scale = absx;
          }

          i++;
        }
      }

      scale = auxdata->tol * muDoubleScalarMax(scale, 1.0);

      /*  Update the solution only if the error is acceptable */
      if (error_1 <= scale) {
        t += h;
        *tout = t;
        for (i = 0; i < 14; i++) {
          x[i] = sol2[i];
          xout[i] = sol2[i];
        }
      }

      /*  Step control */
      if (error_1 == 0.0) {
        error_1 = 2.2204460492503131E-15;
      }

      emlrtPushRtStackR2012b(&c_emlrtRSI, emlrtRootTLSGlobal);
      scale /= error_1;
      emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
      emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
      if (scale < 0.0) {
        emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
        b_eml_error();
        emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
      }

      emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
      emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
      h = muDoubleScalarMin(hmax, 0.9 * h * muDoubleScalarPower(scale, 0.125));
      emlrtPopRtStackR2012b(&c_emlrtRSI, emlrtRootTLSGlobal);
      emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, emlrtRootTLSGlobal);
      if (muDoubleScalarAbs(h) <= 2.2204460492503131E-16) {
        exitg1 = TRUE;
      }
    }
  }
}

/* End of code generation (ode78_.c) */
